import redis
import time
import math
import json
import statistics
import time
import geopy.distance
import pickle
from scipy.stats import expon
import threading
import msgpack
from scipy.spatial.transform import Rotation as R
import msgpack
from skspatial.objects import Points, Plane, Vector
import numpy as np
from skspatial.transformation import transform_coordinates
import subprocess
import serial


class AutoDrive(object):
    def __init__(self):
        self.r = redis.Redis(db=0)
        self.task_index = int(r.get('job_index'))
        self.last_oy = 0
        self.base_speed = 6000
        self.d_queue = []
        self.timestamp = time.time()

        self.distance_to_end_point = 10000  # m
        self.last_gps_timestamp = None
        self.operation_status = None

        self.job_start_location = []
        self.is_new_job = True

        self.pre_direction = 'straight'

        self.ser = serial.Serial('/dev/ttyUSB3')
        with open('/home/asmaa/Documents/asmaa/control/waypoints.json', 'r') as fp:
            self.gps_location = json.load(fp)


        # with open(self.map_file, 'rb') as data_file:
        #     map_data = msgpack.unpack(data_file)
        #
        # keyframe_points = [list(-(R.from_quat(map_data['keyframes'][k]['rot_cw'])).as_matrix().transpose().dot(map_data['keyframes'][k]['trans_cw'])) for k in map_data['keyframes']]
        # points = Points(keyframe_points)
        # plane = Plane.best_fit(points)

        # plane_a = Plane(point=[0, 0, 0], normal=plane.normal)
        # self.v_x = Vector(plane_a.project_point([0, 0, 1])).unit()
        # self.v_z = plane_a.normal
        # self.v_y = Vector(np.cross(self.v_x, self.v_z)).unit()

        # print(self.v_y)
        # print(self.v_x)

        # self.job_start_location = transform_coordinates(
        #     [float(r.get('cam_center_x')), float(r.get('cam_center_y')), float(r.get('cam_center_z'))], [0, 0, 0],
        #     [self.v_x, self.v_y])

    def auto_stop(self):
        self.r.set('forward_left_speed', 0)
        self.r.set('forward_right_speed', 0)

        time.sleep(0.5)

    def new_job(self, stop=True, start_location=False):
        if stop:
            self.auto_stop()

        self.r.lpop('job_queue')

        self.is_new_job = True
        self.pre_direction = 'straight'
        self.d_queue = []

        if start_location:
            self.job_start_location = transform_coordinates([float(r.get('cam_center_x')), float(r.get('cam_center_y')), float(r.get('cam_center_z'))], [0, 0, 0], [self.v_x, self.v_y])

    def auto_straight(self, deviation, max_level):
        if deviation > max_level:
            deviation = max_level

        if deviation < -max_level:
            deviation = -max_level

        self.r.set('forward_left_speed', int(self.base_speed - deviation))
        self.r.set('forward_right_speed', int(self.base_speed + deviation))

        # self.r.set('forward_right_speed', -int(self.base_speed - deviation))
        # self.r.set('forward_left_speed', -int(self.base_speed + deviation))

    def auto_straight_back(self, deviation, max_level):
        if deviation > max_level:
            deviation = max_level

        if deviation < -max_level:
            deviation = -max_level

        self.r.set('forward_left_speed', -int(self.base_speed - deviation))
        self.r.set('forward_right_speed', -int(self.base_speed + deviation))

    def distance_between(self, point_A, point_B):
        return math.sqrt((point_A[0] - point_B[0]) ** 2 + (point_A[1] - point_B[1]) ** 2)

    def line(self, startpoint, endpoint, no_stop):
        current_location = [float(r.get('cam_center_x')), float(r.get('cam_center_y')), float(r.get('cam_center_z'))]
        current_2d = transform_coordinates(current_location, [0, 0, 0], [self.v_x, self.v_y])

        start_2d = transform_coordinates(startpoint, [0, 0, 0], [self.v_x, self.v_y])
        end_2d = transform_coordinates(endpoint, [0, 0, 0], [self.v_x, self.v_y])

        a = self.distance_between(start_2d, current_2d)
        b = self.distance_between(end_2d, current_2d)
        c = self.distance_between(start_2d, end_2d)

        cos = (b ** 2 + c ** 2 - a ** 2) / (2 * b * c)

        p = (a + b + c) / 2

        h = math.sqrt(p * (p - a) * (p - b) * (p - c)) * 2 / c
        # print(h)

        d = (current_2d[0] - start_2d[0]) * (end_2d[1] - start_2d[1]) - (
                current_2d[1] - start_2d[1]) * (end_2d[0] - start_2d[0])

        # print(h, d)

        if d > 0:
            h = -h * 1000
            print('left')
        else:
            h = h * 1000
            print('right')

        print(h)

        self.d_queue = [h] + self.d_queue

        if a < 4:
            self.base_speed = 5200
        elif a < 8:
            self.base_speed = 5200
        elif a > 8:
            self.base_speed = 5200
        else:
            self.base_speed = int(expon.pdf(abs(h) / 100) * 3000)

        print(self.base_speed)

        px = 10
        dx = 40

        if len(self.d_queue) > 15:
            p = h
            d = self.d_queue[0] - self.d_queue[10]
        else:
            p = 0
            d = 0

        deviation = (p * px + d * dx) * 1.8

        self.auto_straight(deviation, 200)

        if len(self.d_queue) > 10:
            self.d_queue = self.d_queue[:15]

        print(self.r.get('forward_left_speed'))
        print(self.r.get('forward_right_speed'))

        if cos < 0:
            if no_stop == 1:
                self.new_job(stop=False)
                print('no stop')
            else:
                print('stop')
                self.auto_stop()
                self.new_job()

    def line_reverse(self, startpoint, endpoint, no_stop):
        current_location = [float(r.get('cam_center_x')), float(r.get('cam_center_y')), float(r.get('cam_center_z'))]
        current_2d = transform_coordinates(current_location, [0, 0, 0], [self.v_x, self.v_y])

        start_2d = transform_coordinates(startpoint, [0, 0, 0], [self.v_x, self.v_y])
        end_2d = transform_coordinates(endpoint, [0, 0, 0], [self.v_x, self.v_y])

        a = self.distance_between(start_2d, current_2d)
        b = self.distance_between(end_2d, current_2d)
        c = self.distance_between(start_2d, end_2d)

        cos = (b ** 2 + c ** 2 - a ** 2) / (2 * b * c)

        p = (a + b + c) / 2

        h = math.sqrt(p * (p - a) * (p - b) * (p - c)) * 2 / c
        # print(h)

        d = (current_2d[0] - start_2d[0]) * (end_2d[1] - start_2d[1]) - (
                current_2d[1] - start_2d[1]) * (end_2d[0] - start_2d[0])

        if d < 0:
            h = -h * 1000
            print('left')
        else:
            h = h * 1000
            print('right')
        delta = 400

        if len(self.d_queue) < 3:
            self.auto_straight_back(0, 2000)
        else:
            pre_h = self.d_queue[0]

            if pre_h - h > 0: #left
                if self.pre_direction == 'go_left' or self.pre_direction == 'straight':
                    if h < 100:
                        self.auto_straight_back(-delta, 3000)
                        self.pre_direction = 'go_right'
                    else:
                        self.auto_straight_back(0, 3000)
                        self.pre_direction = 'straight'
                elif self.pre_direction == 'go_right':
                    self.auto_straight_back(0, 3000)
                    self.pre_direction = 'straight'
            else:
                if self.pre_direction == 'go_right' or self.pre_direction == 'straight':
                    if h > -100:
                        self.auto_straight_back(delta, 3000)
                        self.pre_direction = 'go_left'
                    else:
                        self.auto_straight_back(0, 3000)
                        self.pre_direction = 'straight'
                elif self.pre_direction == 'go_left':
                    self.auto_straight_back(0, 3000)
                    self.pre_direction = 'straight'

        print(h)

        self.d_queue = [h] + self.d_queue

        if a < 4:
            self.base_speed = 6200
        elif a < 8:
            self.base_speed = 6200
        elif a > 8:
            self.base_speed = 6200
        else:
            self.base_speed = int(expon.pdf(abs(h) / 100) * 3000)

        # print(self.base_speed)
        #
        # px = 10
        # dx = 40
        #
        # if len(self.d_queue) > 15:
        #     p = h
        #     d = self.d_queue[0] - self.d_queue[10]
        # else:
        #     p = 0
        #     d = 0
        #
        # deviation = (p * px + d * dx) * 1.8
        #
        # self.auto_straight_back(deviation, 2800)

        if len(self.d_queue) > 10:
            self.d_queue = self.d_queue[:15]

        print(self.r.get('forward_left_speed'))
        print(self.r.get('forward_right_speed'))

        if cos < 0:
            if no_stop == 1:
                self.new_job(stop=False)
                print('no stop')
            else:
                print('stop')
                self.auto_stop()
                self.new_job()

        time.sleep(0.2)

    def fix_speed_turn(self, speed_left, speed_right, time_lock):
        if self.is_new_job:
            self.timestamp = time.time()
            self.is_new_job = False
        elif time.time() - self.timestamp > time_lock:
            self.new_job(stop=True)
        else:
            self.r.set('forward_left_speed', speed_left)
            self.r.set('forward_right_speed', speed_right)

        time.sleep(0.3)

    def roll_down(self):
        r.rpush('command', b'*MOTORA2+#')
        time.sleep(15)
        r.set('radar_1', 'on')
        r.rpush('command', b'*OUT8_ON#')
        time.sleep(1)
        r.rpush('command', b'*OUT8_ON#')
        time.sleep(1)

        r.rpush('command', b'*MOTORA1-#')
        time.sleep(1)
        r.rpush('command', b'*MOTORA1-#')
        self.new_job()

    def roll_up(self):
        r.set('radar_1', 'off')
        time.sleep(1)
        self.ser.write(b'*MOTORA2-#')
        time.sleep(1)
        self.ser.write(b'*MOTORA2-#')
        time.sleep(1)
        self.ser.write(b'*OUT8_OFF#')
        time.sleep(1)
        self.ser.write(b'*OUT8_OFF#')

        self.ser.write(b'*MOTORA1+#')
        time.sleep(1)
        self.ser.write(b'*MOTORA1+#')
        time.sleep(15)
        self.new_job()

    def gps_straight_forward(self, startpoint, endpoint, off_set=-20):
        try:
            start_point = self.gps_location[startpoint]['cor']
            end_point = self.gps_location[endpoint]['cor']

            gps_timestamp = float(self.r.get('gps_timestamp'))
            if (time.time() - gps_timestamp < 300) and (self.last_gps_timestamp != gps_timestamp):
                self.last_gps_timestamp = gps_timestamp
                print('valid gps')

                lat = float(self.r.get('fusion_lat'))
                lon = float(self.r.get('fusion_lon'))
                hAcc = 50

                if hAcc > 100:
                    self.r.set('forward_left_speed', 0)
                    self.r.set('forward_right_speed', 0)
                else:
                    d = (lat - start_point[0]) * (end_point[1] - start_point[1]) - (lon - start_point[1]) * (
                                end_point[0] - start_point[0])

                    a = geopy.distance.distance((start_point[0], start_point[1]), (lat, lon)).m
                    b = geopy.distance.distance((end_point[0], end_point[1]), (lat, lon)).m
                    c = geopy.distance.distance((start_point[0], start_point[1]), (end_point[0], end_point[1])).m

                    print(b)

                    cos = (b ** 2 + c ** 2 - a ** 2) / (2 * b * c)

                    if cos < 0:
                        self.new_job()

                    p = (a + b + c) / 2

                    h = math.sqrt(p * (p - a) * (p - b) * (p - c)) * 2 / c

                    if d > 0:
                        h = -h * 1000
                        print('left')
                    else:
                        h = h * 1000
                        print('right')

                    print(h)

                    self.d_queue = [h] + self.d_queue

                    if a < 3:
                        self.base_speed = 3200
                    else:
                        # self.base_speed = int(expon.pdf(abs(h) / 400) * 4000)
                        self.base_speed = 3200

                    print(self.base_speed)

                    # PID
                    px = 10
                    dx = 80

                    if len(self.d_queue) > 1:
                        p = h + off_set
                        d = self.d_queue[0] - self.d_queue[4]
                    else:
                        p = 0
                        d = 0

                    deviation = (p * px + d * dx) * 1.8

                    self.auto_straight(deviation, 1000)

                    if len(self.d_queue) > 10:
                        self.d_queue = self.d_queue[:10]

            else:
                pass
                # print('not valid')
        except Exception as e:
            print(e)
            pass

    def open_map(self, map_file):
        p = subprocess.Popen(
            '../../openvslam-master/build/run_camera_localization  -v ../../openvslam-master/build/orb_vocab/orb_vocab.dbow2 -n 4 -c ../../openvslam-master/build/asmaa/config_zed.yaml -p ../../openvslam-master/build/%s' % map_file,
            shell=True)

        self.r.set('slam_termination', 0)

        time.sleep(15)

        with open('../../openvslam-master/build/%s' % map_file, 'rb') as data_file:
            map_data = msgpack.unpack(data_file)

        keyframe_points = [list(-(R.from_quat(map_data['keyframes'][k]['rot_cw'])).as_matrix().transpose().dot(map_data['keyframes'][k]['trans_cw'])) for k in map_data['keyframes']]
        points = Points(keyframe_points)
        plane = Plane.best_fit(points)

        plane_a = Plane(point=[0, 0, 0], normal=plane.normal)
        self.v_x = Vector(plane_a.project_point([0, 0, 1])).unit()
        self.v_z = plane_a.normal
        self.v_y = Vector(np.cross(self.v_x, self.v_z)).unit()

        print(self.v_y)
        print(self.v_x)

        self.job_start_location = transform_coordinates(
            [float(r.get('cam_center_x')), float(r.get('cam_center_y')), float(r.get('cam_center_z'))], [0, 0, 0],
            [self.v_x, self.v_y])

        self.new_job(stop=True)

    def close_map(self):
        self.r.set('slam_termination', 1)
        time.sleep(2)
        self.new_job()

    def height_control_on(self):
        self.r.set('radar_1', 'on')

    def height_control_off(self):
        self.r.set('radar_1', 'off')

    def set_threshold(self, threshold):
        self.r.set('threshold', threshold)
        self.new_job()

    def record_gps(self, name):
        lat = float(self.r.get('fusion_lat'))
        lon = float(self.r.get('fusion_lon'))

        self.gps_location[name] = {'cor': [lat, lon]}

        with open('/home/asmaa/Documents/asmaa/control/waypoints.json', 'w') as fp:
            json.dump(self.gps_location, fp)

        self.new_job()

    def auto_task(self):
        job_queue = [
            ['line', [[0.375, -0.133, -0.215], [-1.563, 8.766, -35.902]]]
        ]

        function_dict = {
            'openvslam_line': self.line,
            'fix_speed': self.fix_speed_turn,
            'map_open': self.open_map,
            'map_close': self.close_map,

            'hcontrol_on': self.height_control_on,
            'hcontrol_off': self.height_control_off,
            'roll_up': self.roll_up,
            'roll_down': self.roll_down,
            'set_th': self.set_threshold,
            'line': self.gps_straight_forward,
            'reverse_line': self.line_reverse,

            'record_gps': self.record_gps,
        }

        if self.r.llen('job_queue') > 0:
        # if len(job_queue) > 0:
            job = json.loads(r.lindex('job_queue', 0))
            # job = job_queue[0]
            function_dict[job[0]](*job[1])
        else:
            self.auto_stop()
            # self.restart_sensor(post_detect_on=False, camera_1_on=False)


if __name__ == '__main__':
    r = redis.Redis(db=0)
    r.set('process_6', 'on')

    autoDrive = AutoDrive()

    while r.get('process_6').decode("utf-8") == 'on':
        try:
            autoDrive.auto_task()
            time.sleep(0.1)
        except KeyboardInterrupt:
            autoDrive.auto_stop()
            break
